#include "IOmapping.h"
#include <math.h>
#include <string.h>

static bool IsBitSet(int data, int index) { return (data & (1 << index)) != 0; }

void update_pn_status()
{
    // zucmot_joint_t* joint;

    // if (read_IODevState(IODevTYPE_SYNCH_PN_DEV) == 101)
    // {
    //     // profinet robot serial nummber
    //     write_Robot_i(IODevTYPE_SYNCH_PN_DEV, 0, zucmotStatus->servo_status.serial_num);

    //     // profinet servo version
    //     write_Robot_i(IODevTYPE_SYNCH_PN_DEV, 1, zucmotStatus->servo_status.servo_version);

    //     // @FIXME liwang  7轴需求进行修改
    //     for (int idx = 0; idx < 6; idx++)
    //     {
    //         joint = &(zucmotDebug->joints[idx]);

    //         // profinet servo voltage
    //         write_Joint_f(IODevTYPE_SYNCH_PN_DEV, idx, zucmotStatus->servo_status.jointMonitorData[idx].instVoltage);

    //         // profinet joint current
    //         write_Joint_f(IODevTYPE_SYNCH_PN_DEV, 6 + idx, zucmotStatus->servo_status.jointMonitorData[idx].instCurrent);

    //         // profinet joint position
    //         write_Joint_f(IODevTYPE_SYNCH_PN_DEV, 12 + idx, joint->pos_cmd);

    //         // profinet joint vel
    //         write_Joint_f(IODevTYPE_SYNCH_PN_DEV, 18 + idx, joint->vel_cmd);

    //         // profinet servo temperature
    //         write_Joint_f(IODevTYPE_SYNCH_PN_DEV, 24 + idx, zucmotStatus->servo_status.jointMonitorData[idx].instTemperature);

    //         // profinet joint torque
    //         write_Joint_f(IODevTYPE_SYNCH_PN_DEV, 30 + idx, zucmotStatus->servo_status.jointMonitorData[idx].jointTorque);

    //         // profinet servo error code
    //         if (((zucmot_hal_data->error_triggered == true && (zucmotStatus->servo_status.errorCode[idx] != 0)) ||
    //              (zucmot_hal_data->error_triggered == false && (zucmotStatus->servo_status.errorCode[idx] == 0))))
    //         {
    //             write_Joint_int32(IODevTYPE_SYNCH_PN_DEV, idx, zucmotStatus->servo_status.errorCode[idx]);
    //         }
    //         if (((zucmot_hal_data->error_triggered == true && (zucmotStatus->servo_status.collisionState[idx] != 0)) ||
    //              (zucmot_hal_data->error_triggered == false && (zucmotStatus->servo_status.collisionState[idx] == 0))))
    //         {
    //             uint8_t tmp = 0;
    //             read_Joint_uint8(IODevTYPE_SYNCH_PN_DEV, 2, &tmp);
    //             tmp |= tmp & (zucmotStatus->servo_status.collisionState[idx]) << idx;
    //             write_Joint_uint8(IODevTYPE_SYNCH_PN_DEV, 2, tmp);
    //         }

    //         // profinet error state
    //         uint8_t tmp0 = 0;
    //         read_Joint_uint8(IODevTYPE_SYNCH_PN_DEV, 0, &tmp0);
    //         tmp0 |= tmp0 & (zucmotStatus->servo_status.errorState[idx]) << idx;
    //         write_Joint_uint8(IODevTYPE_SYNCH_PN_DEV, 0, tmp0);

    //         uint8_t tmp1 = 0;
    //         read_Joint_uint8(IODevTYPE_SYNCH_PN_DEV, 1, &tmp1);
    //         tmp1 |= tmp1 & (zucmotStatus->servo_status.enableState[idx]) << idx;
    //         write_Joint_uint8(IODevTYPE_SYNCH_PN_DEV, 1, tmp1);
    //     }
    //     for (int idx = 0; idx < 6; idx++)
    //     {
    //         // profinet tcp_pos
    //         write_TCP_f(IODevTYPE_SYNCH_PN_DEV, 0 + idx, zucmotStatus->tcp_pos[idx]);

    //         // profinet tcp_vel
    //         write_TCP_f(IODevTYPE_SYNCH_PN_DEV, 6 + idx, zucmotStatus->vel_tcp[idx]);

    //         // write_TCP_f(IODevTYPE_SYNCH_PN_DEV, 6 + idx, zucmotStatus->vel_tcp_angular[idx]);

    //         // profinet tcp_offset
    //         write_TCP_f(IODevTYPE_SYNCH_PN_DEV, 12 + idx, zucmotStatus->tool_offset[idx]);

    //         // profinet base_offset
    //         write_BaseOffset_f(IODevTYPE_SYNCH_PN_DEV, 0 + idx, zucmotStatus->user_offset[idx]);
    //     }

    //     if (zucmotStatus->protective_stop == 1)
    //     {
    //         uint8_t tmp2 = 0;
    //         read_SafetyOUT_uint8(IODevTYPE_SYNCH_PN_DEV, 2, &tmp2);
    //         tmp2 |= (0xff & (zucmotStatus->protective_stop << 1));
    //         write_SafetyOUT_uint8(IODevTYPE_SYNCH_PN_DEV, 2, tmp2);
    //     }
    //     else
    //     {
    //         uint8_t tmp2 = 0;
    //         read_SafetyOUT_uint8(IODevTYPE_SYNCH_PN_DEV, 2, &tmp2);
    //         tmp2 &= ~(0xff & (1 << 1));
    //         write_SafetyOUT_uint8(IODevTYPE_SYNCH_PN_DEV, 2, tmp2);
    //     }

    //     if (zucmotStatus->emergency_stop == 1)
    //     {
    //         uint8_t tmp2 = 0;
    //         read_SafetyOUT_uint8(IODevTYPE_SYNCH_PN_DEV, 2, &tmp2);
    //         tmp2 |= (0xff & zucmotStatus->emergency_stop);
    //         write_SafetyOUT_uint8(IODevTYPE_SYNCH_PN_DEV, 2, tmp2);
    //     }
    //     else
    //     {
    //         uint8_t tmp2 = 0;
    //         read_SafetyOUT_uint8(IODevTYPE_SYNCH_PN_DEV, 2, &tmp2);
    //         tmp2 &= ~(0xff & 1);
    //         write_SafetyOUT_uint8(IODevTYPE_SYNCH_PN_DEV, 2, tmp2);
    //     }
    //     if (zucmotStatus->on_soft_limit == 1)
    //     {
    //         uint8_t tmp2 = 0;
    //         read_SafetyOUT_uint8(IODevTYPE_SYNCH_PN_DEV, 2, &tmp2);
    //         tmp2 |= (0xff & (zucmotStatus->on_soft_limit << 2));
    //         write_SafetyOUT_uint8(IODevTYPE_SYNCH_PN_DEV, 2, tmp2);
    //     }
    //     else
    //     {
    //         uint8_t tmp2 = 0;
    //         read_SafetyOUT_uint8(IODevTYPE_SYNCH_PN_DEV, 2, &tmp2);
    //         tmp2 &= ~(0xff & (1 << 2));
    //         write_SafetyOUT_uint8(IODevTYPE_SYNCH_PN_DEV, 2, tmp2);
    //     }

    //     write_SafetyOUT_uint8(IODevTYPE_SYNCH_PN_DEV, 1, zucmotStatus->percentage_mode_level);

    //     //profinet motion mode, motion error code
    //     write_SafetyOUT_uint8(IODevTYPE_SYNCH_PN_DEV, 0, zucmotStatus->motion_mode);
    //     write_SafetyOUT_int32(IODevTYPE_SYNCH_PN_DEV, 0, zucmot_hal_data->motion_error_code);

    //     // profinet checksum
    //     write_SafetyOUT_int32(IODevTYPE_SYNCH_PN_DEV, 1, zucmotStatus->safetyCRC);

    //     //profinet power_on
    //     if (zucmotStatus->powered_on == 1)
    //     {
    //         uint8_t tmp0 = 0;
    //         read_Robot_uint8(IODevTYPE_SYNCH_PN_DEV, 0, &tmp0);
    //         tmp0 |= (0xff & zucmotStatus->powered_on);
    //         write_Robot_uint8(IODevTYPE_SYNCH_PN_DEV, 0, tmp0);
    //     }
    //     else
    //     {
    //         uint8_t tmp0 = 0;
    //         read_Robot_uint8(IODevTYPE_SYNCH_PN_DEV, 0, &tmp0);
    //         tmp0 &= ~(0xff & 1);
    //         write_Robot_uint8(IODevTYPE_SYNCH_PN_DEV, 0, tmp0);
    //     }
    //     if (GET_MOTION_ENABLE_FLAG())
    //     {
    //         uint8_t tmp0 = 0;
    //         read_Robot_uint8(IODevTYPE_SYNCH_PN_DEV, 0, &tmp0);
    //         tmp0 |= (0xff & (GET_MOTION_ENABLE_FLAG() << 1));
    //         write_Robot_uint8(IODevTYPE_SYNCH_PN_DEV, 0, tmp0);
    //     }
    //     else
    //     {
    //         uint8_t tmp0 = 0;
    //         read_SafetyOUT_uint8(IODevTYPE_SYNCH_PN_DEV, 0, &tmp0);
    //         tmp0 &= ~(0xff & (1 << 1));
    //         write_SafetyOUT_uint8(IODevTYPE_SYNCH_PN_DEV, 0, tmp0);
    //     }

    //     //profinet cabinet temperature
    //     write_Robot_f(IODevTYPE_SYNCH_PN_DEV, 2, zucmotStatus->cab_status.cabTemperature);

    //     //profinet cabinet average current
    //     write_Robot_f(IODevTYPE_SYNCH_PN_DEV, 0, zucmotStatus->cab_status.robotAverageCurrent);

    //     //profinet cabinet average
    //     write_Robot_f(IODevTYPE_SYNCH_PN_DEV, 1, zucmotStatus->cab_status.robotAveragePower);
    // }

    // if (read_IODevEnable(IODevTYPE_SYNCH_PN_DEV) == 1)
    // {
    //     zucmotStatus->profinet_enable = read_IODevEnable(IODevTYPE_SYNCH_PN_DEV);
    //     zucmotStatus->profinet_state = read_IODevState(IODevTYPE_SYNCH_PN_DEV);

    //     // profinet DIO
    //     for (int i = 0; i < ZUCMOT_MAX_PNDev_DIO; ++i)
    //     {
    //         int idx = zucmotConfig->cabUsrPhysNumDIO + ZUCMOT_MAX_MODBUS_DIO + i;
    //         Read_DIOStatus_i(IODevTYPE_SYNCH_PN_DEV, ADIO_INPUT, i, &zucmotStatus->synch_pn_dev_di[i]);
    //         Read_DIOStatus_i(IODevTYPE_SYNCH_PN_DEV, ADIO_OUTPUT, i, &zucmotStatus->synch_pn_dev_do[i]);

    //         Read_DIOStatus_i(IODevTYPE_SYNCH_PN_DEV, ADIO_INPUT, i, &zucmotStatus->synch_di[idx]);
    //         Read_DIOStatus_i(IODevTYPE_SYNCH_PN_DEV, ADIO_OUTPUT, i, &zucmotStatus->synch_do[idx]);
    //     }
    //     // Profinet AIO
    //     for (int i = 0; i < ZUCMOT_MAX_PNDev_AIO_NUM; ++i)
    //     {
    //         int idx = zucmotConfig->cabUsrPhysNumAI + ZUCMOT_MAX_MODBUS_AIO_NUM + i;
    //         Read_AIOStatus_f(IODevTYPE_SYNCH_PN_DEV, ADIO_INPUT, i, &zucmotStatus->analog_pn_dev_input[i]);
    //         Read_AIOStatus_f(IODevTYPE_SYNCH_PN_DEV, ADIO_OUTPUT, i, &zucmotStatus->analog_pn_dev_output[i]);

    //         Read_AIOStatus_f(IODevTYPE_SYNCH_PN_DEV, ADIO_INPUT, i, &zucmotStatus->analog_input[idx]);
    //         Read_AIOStatus_f(IODevTYPE_SYNCH_PN_DEV, ADIO_OUTPUT, i, &zucmotStatus->analog_output[idx]);
    //     }
    // }
}

void update_eip_status()
{
    // zucmot_joint_t* joint;

    // if (read_IODevState(IODevTYPE_SYNCH_EIP_ADPT) == 4)
    // {
    //     // ethernet/ip robot serial nummber
    //     write_Robot_i(IODevTYPE_SYNCH_EIP_ADPT, 0, zucmotStatus->servo_status.serial_num);
    //     //  zucmotEIPItf->eipData.ROBOT_OUT_INT[0] = zucmotStatus->servo_status.serial_num;

    //     // ethernet/ip servo version
    //     write_Robot_i(IODevTYPE_SYNCH_EIP_ADPT, 1, zucmotStatus->servo_status.servo_version);
    //     //  zucmotEIPItf->eipData.ROBOT_OUT_INT[1] = zucmotStatus->servo_status.servo_version;

    //     // @FIXME liwang  7轴需求进行修改
    //     for (int idx = 0; idx < 6; idx++)
    //     {
    //         joint = &(zucmotDebug->joints[idx]);
    //         // ethernet/ip joint current
    //         write_Joint_f(IODevTYPE_SYNCH_EIP_ADPT, idx, zucmotStatus->servo_status.jointMonitorData[idx].instCurrent);
    //         // ethernet/ip joint position
    //         write_Joint_f(IODevTYPE_SYNCH_EIP_ADPT, 6 + idx, joint->pos_cmd);

    //         // ethernet/ip joint vel
    //         write_Joint_f(IODevTYPE_SYNCH_EIP_ADPT, 12 + idx, joint->vel_cmd);

    //         // ethernet/ip joint torque
    //         write_Joint_f(IODevTYPE_SYNCH_EIP_ADPT, 18 + idx, zucmotStatus->servo_status.jointMonitorData[idx].jointTorque);

    //         // ethernet/ip servo error code
    //         if (((zucmot_hal_data->error_triggered == true && (zucmotStatus->servo_status.errorCode[idx] != 0)) ||
    //              (zucmot_hal_data->error_triggered == false && (zucmotStatus->servo_status.errorCode[idx] == 0))))
    //         {
    //             write_Joint_int32(IODevTYPE_SYNCH_EIP_ADPT, idx, zucmotStatus->servo_status.errorCode[idx]);
    //         }
    //         if (((zucmot_hal_data->error_triggered == true && (zucmotStatus->servo_status.collisionState[idx] != 0)) ||
    //              (zucmot_hal_data->error_triggered == false && (zucmotStatus->servo_status.collisionState[idx] == 0))))
    //         {
    //             uint8_t tmp = 0;
    //             read_Joint_uint8(IODevTYPE_SYNCH_EIP_ADPT, 2, &tmp);
    //             tmp |= tmp & (zucmotStatus->servo_status.collisionState[idx]) << idx;
    //             write_Joint_uint8(IODevTYPE_SYNCH_EIP_ADPT, 2, tmp);
    //         }

    //         // ethernet/ip error state
    //         uint8_t tmp0 = 0;
    //         read_Joint_uint8(IODevTYPE_SYNCH_EIP_ADPT, 0, &tmp0);
    //         tmp0 |= tmp0 & (zucmotStatus->servo_status.errorState[idx]) << idx;
    //         write_Joint_uint8(IODevTYPE_SYNCH_EIP_ADPT, 0, tmp0);

    //         uint8_t tmp1 = 0;
    //         read_Joint_uint8(IODevTYPE_SYNCH_EIP_ADPT, 1, &tmp1);
    //         tmp1 |= tmp1 & (zucmotStatus->servo_status.enableState[idx]) << idx;
    //         write_Joint_uint8(IODevTYPE_SYNCH_EIP_ADPT, 1, tmp1);
    //     }
    //     for (int idx = 0; idx < 6; idx++)
    //     {
    //         // ethernet/ip sensor torque
    //         write_TCP_f(IODevTYPE_SYNCH_EIP_ADPT, idx, zucmotStatus->actualTorque[idx]);

    //         // ethernet/ip tcp_pos
    //         write_TCP_f(IODevTYPE_SYNCH_EIP_ADPT, 6 + idx, zucmotStatus->tcp_pos[idx]);

    //         // ethernet/ip tcp_offset
    //         write_TCP_f(IODevTYPE_SYNCH_EIP_ADPT, 12 + idx, zucmotStatus->tool_offset[idx]);

    //         // ethernet/ip tcp_vel
    //         write_TCP_f(IODevTYPE_SYNCH_EIP_ADPT, 18, zucmotStatus->vel_tcp_linear);

    //         write_TCP_f(IODevTYPE_SYNCH_EIP_ADPT, 19, zucmotStatus->vel_tcp_angular);
    //     }

    //     if (zucmotStatus->protective_stop == 1)
    //     {
    //         uint8_t tmp2 = 0;
    //         read_SafetyOUT_uint8(IODevTYPE_SYNCH_EIP_ADPT, 2, &tmp2);
    //         tmp2 |= (0xff & (zucmotStatus->protective_stop << 1));
    //         write_SafetyOUT_uint8(IODevTYPE_SYNCH_EIP_ADPT, 2, tmp2);
    //     }
    //     else
    //     {
    //         uint8_t tmp2 = 0;
    //         read_SafetyOUT_uint8(IODevTYPE_SYNCH_EIP_ADPT, 2, &tmp2);
    //         tmp2 &= ~(0xff & (1 << 1));
    //         write_SafetyOUT_uint8(IODevTYPE_SYNCH_EIP_ADPT, 2, tmp2);
    //     }

    //     if (zucmotStatus->emergency_stop == 1)
    //     {
    //         uint8_t tmp2 = 0;
    //         read_SafetyOUT_uint8(IODevTYPE_SYNCH_EIP_ADPT, 2, &tmp2);
    //         tmp2 |= (0xff & zucmotStatus->emergency_stop);
    //         write_SafetyOUT_uint8(IODevTYPE_SYNCH_EIP_ADPT, 2, tmp2);
    //     }
    //     else
    //     {
    //         uint8_t tmp2 = 0;
    //         read_SafetyOUT_uint8(IODevTYPE_SYNCH_EIP_ADPT, 2, &tmp2);
    //         tmp2 &= ~(0xff & 1);
    //         write_SafetyOUT_uint8(IODevTYPE_SYNCH_EIP_ADPT, 2, tmp2);
    //     }
    //     if (zucmotStatus->on_soft_limit == 1)
    //     {
    //         uint8_t tmp2 = 0;
    //         read_SafetyOUT_uint8(IODevTYPE_SYNCH_EIP_ADPT, 2, &tmp2);
    //         tmp2 |= (0xff & (zucmotStatus->on_soft_limit << 2));
    //         write_SafetyOUT_uint8(IODevTYPE_SYNCH_EIP_ADPT, 2, tmp2);
    //     }
    //     else
    //     {
    //         uint8_t tmp2 = 0;
    //         read_SafetyOUT_uint8(IODevTYPE_SYNCH_EIP_ADPT, 2, &tmp2);
    //         tmp2 &= ~(0xff & (1 << 2));
    //         write_SafetyOUT_uint8(IODevTYPE_SYNCH_EIP_ADPT, 2, tmp2);
    //     }

    //     write_SafetyOUT_uint8(IODevTYPE_SYNCH_EIP_ADPT, 1, zucmotStatus->percentage_mode_level);

    //     //ethernet/ip motion mode, motion error code
    //     write_SafetyOUT_uint8(IODevTYPE_SYNCH_EIP_ADPT, 0, zucmotStatus->motion_mode);
    //     write_SafetyOUT_int32(IODevTYPE_SYNCH_EIP_ADPT, 0, zucmot_hal_data->motion_error_code);

    //     // ethernet/ip checksum
    //     write_SafetyOUT_int32(IODevTYPE_SYNCH_EIP_ADPT, 1, zucmotStatus->safetyCRC);

    //     //ethernet/ip power_on
    //     if (zucmotStatus->powered_on == 1)
    //     {
    //         uint8_t tmp0 = 0;
    //         read_Robot_uint8(IODevTYPE_SYNCH_EIP_ADPT, 0, &tmp0);
    //         tmp0 |= (0xff & zucmotStatus->powered_on);
    //         write_Robot_uint8(IODevTYPE_SYNCH_EIP_ADPT, 0, tmp0);
    //     }
    //     else
    //     {
    //         uint8_t tmp0 = 0;
    //         read_Robot_uint8(IODevTYPE_SYNCH_EIP_ADPT, 0, &tmp0);
    //         tmp0 &= ~(0xff & 1);
    //         write_Robot_uint8(IODevTYPE_SYNCH_EIP_ADPT, 0, tmp0);
    //     }
    //     if (GET_MOTION_ENABLE_FLAG())
    //     {
    //         uint8_t tmp0 = 0;
    //         read_Robot_uint8(IODevTYPE_SYNCH_EIP_ADPT, 0, &tmp0);
    //         tmp0 |= (0xff & (GET_MOTION_ENABLE_FLAG() << 1));
    //         write_Robot_uint8(IODevTYPE_SYNCH_EIP_ADPT, 0, tmp0);
    //     }
    //     else
    //     {
    //         uint8_t tmp0 = 0;
    //         read_SafetyOUT_uint8(IODevTYPE_SYNCH_EIP_ADPT, 0, &tmp0);
    //         tmp0 &= ~(0xff & (1 << 1));
    //         write_SafetyOUT_uint8(IODevTYPE_SYNCH_EIP_ADPT, 0, tmp0);
    //     }
    // }

    // // ethernet/IP DIO
    // if (read_IODevEnable(IODevTYPE_SYNCH_EIP_ADPT) == 1)
    // {
    //     zucmotStatus->eip_enable = read_IODevEnable(IODevTYPE_SYNCH_EIP_ADPT);
    //     zucmotStatus->eip_state = read_IODevState(IODevTYPE_SYNCH_EIP_ADPT);
    //     if (zucmotEIPStruct->mode == 1)
    //     {
    //         zucmotStatus->eip_mode = zucmotEIPStruct->mode;
    //         zucmotStatus->eip_di_num = zucmotEIPStruct->di_num;
    //         zucmotStatus->eip_do_num = zucmotEIPStruct->do_num;
    //         zucmotStatus->eip_ai_int_num = zucmotEIPStruct->ai_int_num;
    //         zucmotStatus->eip_ai_float_num = zucmotEIPStruct->ai_float_num;
    //         zucmotStatus->eip_ao_int_num = zucmotEIPStruct->ao_int_num;
    //         zucmotStatus->eip_ao_float_num = zucmotEIPStruct->ao_float_num;
    //         memcpy(zucmotStatus->synch_eip_in, zucmotEIPStruct->eipModeData.EIP_IN, 492);
    //         memcpy(zucmotStatus->synch_eip_out, zucmotEIPStruct->eipModeData.EIP_OUT, 492);
    //     }
    //     else
    //     {
    //         for (int i = 0; i < ZUCMOT_MAX_EIP_DIO; ++i)
    //         {
    //             Read_DIOStatus_i(IODevTYPE_SYNCH_EIP_ADPT, ADIO_INPUT, i, &zucmotStatus->synch_eip_adpt_di[i]);
    //             Read_DIOStatus_i(IODevTYPE_SYNCH_EIP_ADPT, ADIO_OUTPUT, i, &zucmotStatus->synch_eip_adpt_do[i]);
    //         }
    //         // Ethernet/IP AIO
    //         for (int i = 0; i < ZUCMOT_MAX_EIP_AIO_NUM; ++i)
    //         {
    //             Read_AIOStatus_f(IODevTYPE_SYNCH_EIP_ADPT, ADIO_INPUT, i, &zucmotStatus->analog_eip_adpt_input[i]);
    //             Read_AIOStatus_f(IODevTYPE_SYNCH_EIP_ADPT, ADIO_OUTPUT, i, &zucmotStatus->analog_eip_adpt_output[i]);
    //         }
    //     }
    // }
}

void set_physic_IO_status()
{
    // if (zucmotConfig->cab_type > 1)
    // {
    //     //cab physic do
    //     int phyIOstat[16] = {0};
    //     for (int i = 0; i < 16; i++) { Read_DIOStatus_i(IODevTYPE_PHYSIC, ADIO_OUTPUT, i, &phyIOstat[i]); }

    //     cab_internal_info.scb_do_ctl.do_signal.user_do_1 = phyIOstat[0];
    //     cab_internal_info.scb_do_ctl.do_signal.user_do_2 = phyIOstat[1];
    //     cab_internal_info.scb_do_ctl.do_signal.user_do_3 = phyIOstat[2];
    //     cab_internal_info.scb_do_ctl.do_signal.user_do_4 = phyIOstat[3];
    //     cab_internal_info.scb_do_ctl.do_signal.user_do_5 = phyIOstat[4];
    //     cab_internal_info.scb_do_ctl.do_signal.user_do_6 = phyIOstat[5];
    //     cab_internal_info.scb_do_ctl.do_signal.user_do_7 = phyIOstat[6];
    //     cab_internal_info.scb_do_ctl.do_signal.user_do_8 = phyIOstat[7];
    //     cab_internal_info.scb_do_ctl.do_signal.user_do_9 = phyIOstat[8];
    //     cab_internal_info.scb_do_ctl.do_signal.user_do_10 = phyIOstat[9];
    //     cab_internal_info.scb_do_ctl.do_signal.user_do_11 = phyIOstat[10];
    //     cab_internal_info.scb_do_ctl.do_signal.user_do_12 = phyIOstat[11];
    //     cab_internal_info.scb_do_ctl.do_signal.user_do_13 = phyIOstat[12];
    //     cab_internal_info.scb_do_ctl.do_signal.user_do_14 = phyIOstat[13];
    //     cab_internal_info.scb_do_ctl.do_signal.user_do_15 = phyIOstat[14];
    //     cab_internal_info.scb_do_ctl.do_signal.user_do_16 = phyIOstat[15];

    //     if (zucmotConfig->cab_type == 2)
    //     {
    //         //cab physic ao
    //         union UN_Float tmp = {0};
    //         Read_AIOStatus_f(IODevTYPE_PHYSIC, ADIO_OUTPUT, 0, &tmp.PN_FLT);

    //         cab_internal_info.scb_ao_ctl.ao_signal.ao_data_1_h = tmp.PN_UI8[1];
    //         cab_internal_info.scb_ao_ctl.ao_signal.ao_data_1_l = tmp.PN_UI8[0];
    //         Read_AIOStatus_f(IODevTYPE_PHYSIC, ADIO_OUTPUT, 1, &tmp.PN_FLT);
    //         cab_internal_info.scb_ao_ctl.ao_signal.ao_data_2_h = tmp.PN_UI8[1];
    //         cab_internal_info.scb_ao_ctl.ao_signal.ao_data_2_l = tmp.PN_UI8[0];

    //         // cab_internal_info.scb_ao_ctl.ao_signal.ao_data_1_h = zucmotPhysicIOStruct->physic_cab_ao.scb_ao_ctl.data_1_h;
    //         // cab_internal_info.scb_ao_ctl.ao_signal.ao_data_1_l = zucmotPhysicIOItf->physic_cab_ao.scb_ao_ctl.data_1_l;
    //         // cab_internal_info.scb_ao_ctl.ao_signal.ao_data_2_h = zucmotPhysicIOItf->physic_cab_ao.scb_ao_ctl.data_2_h;
    //         // cab_internal_info.scb_ao_ctl.ao_signal.ao_data_2_l = zucmotPhysicIOItf->physic_cab_ao.scb_ao_ctl.data_2_l;
    //     }
    // }
    // else
    // {
    //     //cab physic do
    //     int pyhIOstate = 0;
    //     for (int dio = 0; dio < zucmotConfig->cabUsrPhysNumDIO; dio++)
    //     {
    //         Read_DIOStatus_i(IODevTYPE_PHYSIC, ADIO_OUTPUT, dio, &pyhIOstate);
    //         zucmot_hal_data->physical_do[dio] = pyhIOstate;
    //     }
    //     // cab v1.0 has no ao
    // }
}

#define TRACKING_MAX_COM_NUM 15
static int first_di5_open = 1;
static long int deltaCnts[TRACKING_MAX_COM_NUM] = {0};
static int deltaCntIndex = 0;
void update_phy_IO()
{
    // int dio, aio;
    // // cabinet digital IO
    // if (zucmotConfig->cab_type == 1)
    // {
    //     for (dio = 0; dio < zucmotConfig->cabUsrPhysNumDIO; dio++)
    //     {
    //         zucmotStatus->synch_di[dio] = zucmot_hal_data->physical_di[dio];
    //         zucmotStatus->synch_do[dio] = zucmot_hal_data->physical_do[dio];
    //     }
    // }
    // else if (zucmotConfig->cab_type == 2)
    // {
    //     //cab physic IO synch to zucmotStatus
    //     zucmotStatus->synch_di[0] = cab_internal_info.scb_di_uhi_info.di_signal.user_di_1;
    //     zucmotStatus->synch_di[1] = cab_internal_info.scb_di_uhi_info.di_signal.user_di_2;
    //     zucmotStatus->synch_di[2] = cab_internal_info.scb_di_uhi_info.di_signal.user_di_3;
    //     zucmotStatus->synch_di[3] = cab_internal_info.scb_di_uhi_info.di_signal.user_di_4;
    //     zucmotStatus->synch_di[4] = cab_internal_info.scb_di_uhi_info.di_signal.user_di_5;
    //     zucmotStatus->synch_di[5] = cab_internal_info.scb_di_uhi_info.di_signal.user_di_6;
    //     zucmotStatus->synch_di[6] = cab_internal_info.scb_di_uhi_info.di_signal.user_di_7;
    //     zucmotStatus->synch_di[7] = cab_internal_info.scb_di_uhi_info.di_signal.user_di_8;
    //     zucmotStatus->synch_di[8] = cab_internal_info.scb_di_uhi_info.di_signal.user_di_9;
    //     zucmotStatus->synch_di[9] = cab_internal_info.scb_di_uhi_info.di_signal.user_di_10;
    //     zucmotStatus->synch_di[10] = cab_internal_info.scb_di_uhi_info.di_signal.user_di_11;
    //     zucmotStatus->synch_di[11] = cab_internal_info.scb_di_uhi_info.di_signal.user_di_12;
    //     zucmotStatus->synch_di[12] = cab_internal_info.scb_di_uhi_info.di_signal.user_di_13;
    //     zucmotStatus->synch_di[13] = cab_internal_info.scb_di_uhi_info.di_signal.user_di_14;
    //     zucmotStatus->synch_di[14] = cab_internal_info.scb_di_uhi_info.di_signal.user_di_15;
    //     zucmotStatus->synch_di[15] = cab_internal_info.scb_di_uhi_info.di_signal.user_di_16;
    //     zucmotStatus->synch_do[0] = cab_internal_info.scb_do_ao_feedback_info.do_ao_feedback.user_do_1;
    //     zucmotStatus->synch_do[1] = cab_internal_info.scb_do_ao_feedback_info.do_ao_feedback.user_do_2;
    //     zucmotStatus->synch_do[2] = cab_internal_info.scb_do_ao_feedback_info.do_ao_feedback.user_do_3;
    //     zucmotStatus->synch_do[3] = cab_internal_info.scb_do_ao_feedback_info.do_ao_feedback.user_do_4;
    //     zucmotStatus->synch_do[4] = cab_internal_info.scb_do_ao_feedback_info.do_ao_feedback.user_do_5;
    //     zucmotStatus->synch_do[5] = cab_internal_info.scb_do_ao_feedback_info.do_ao_feedback.user_do_6;
    //     zucmotStatus->synch_do[6] = cab_internal_info.scb_do_ao_feedback_info.do_ao_feedback.user_do_7;
    //     zucmotStatus->synch_do[7] = cab_internal_info.scb_do_ao_feedback_info.do_ao_feedback.user_do_8;
    //     zucmotStatus->synch_do[8] = cab_internal_info.scb_do_ao_feedback_info.do_ao_feedback.user_do_9;
    //     zucmotStatus->synch_do[9] = cab_internal_info.scb_do_ao_feedback_info.do_ao_feedback.user_do_10;
    //     zucmotStatus->synch_do[10] = cab_internal_info.scb_do_ao_feedback_info.do_ao_feedback.user_do_11;
    //     zucmotStatus->synch_do[11] = cab_internal_info.scb_do_ao_feedback_info.do_ao_feedback.user_do_12;
    //     zucmotStatus->synch_do[12] = cab_internal_info.scb_do_ao_feedback_info.do_ao_feedback.user_do_13;
    //     zucmotStatus->synch_do[13] = cab_internal_info.scb_do_ao_feedback_info.do_ao_feedback.user_do_14;
    //     zucmotStatus->synch_do[14] = cab_internal_info.scb_do_ao_feedback_info.do_ao_feedback.user_do_15;
    //     zucmotStatus->synch_do[15] = cab_internal_info.scb_do_ao_feedback_info.do_ao_feedback.user_do_16;
    // }
    // else if (zucmotConfig->cab_type == 3)
    // {
    //     int dio_type = cab_internal_info.scb_ai_uhi_info.pscb_ai_signal.dio_type;
    //     zucmotStatus->synch_di[0] = cab_internal_info.scb_di_uhi_info.pscb_di_signal.user_di_1 & IsBitSet(dio_type, 0);
    //     zucmotStatus->synch_di[1] = cab_internal_info.scb_di_uhi_info.pscb_di_signal.user_di_2 & IsBitSet(dio_type, 1);
    //     zucmotStatus->synch_di[2] = cab_internal_info.scb_di_uhi_info.pscb_di_signal.user_di_3 & IsBitSet(dio_type, 2);
    //     zucmotStatus->synch_di[3] = cab_internal_info.scb_di_uhi_info.pscb_di_signal.user_di_4 & IsBitSet(dio_type, 3);
    //     zucmotStatus->synch_di[4] = cab_internal_info.scb_di_uhi_info.pscb_di_signal.user_di_5 & IsBitSet(dio_type, 4);
    //     zucmotStatus->synch_di[5] = cab_internal_info.scb_di_uhi_info.pscb_di_signal.user_di_6 & IsBitSet(dio_type, 5);
    //     zucmotStatus->synch_di[6] = cab_internal_info.scb_di_uhi_info.pscb_di_signal.user_di_7 & IsBitSet(dio_type, 6);
    //     zucmotStatus->synch_do[0] = cab_internal_info.scb_do_ao_feedback_info.pscb_do_feedback.user_do_1;
    //     zucmotStatus->synch_do[1] = cab_internal_info.scb_do_ao_feedback_info.pscb_do_feedback.user_do_2;
    //     zucmotStatus->synch_do[2] = cab_internal_info.scb_do_ao_feedback_info.pscb_do_feedback.user_do_3;
    //     zucmotStatus->synch_do[3] = cab_internal_info.scb_do_ao_feedback_info.pscb_do_feedback.user_do_4;
    //     zucmotStatus->synch_do[4] = cab_internal_info.scb_do_ao_feedback_info.pscb_do_feedback.user_do_5;
    //     zucmotStatus->synch_do[5] = cab_internal_info.scb_do_ao_feedback_info.pscb_do_feedback.user_do_6;
    //     zucmotStatus->synch_do[6] = cab_internal_info.scb_do_ao_feedback_info.pscb_do_feedback.user_do_7;
    // }
    // //  fixmecabv3
    // zucmotStatus->di_type_pin_g1 = cab_internal_info.scb_di_uhi_info.di_signal.io_type_bit0 + (cab_internal_info.scb_di_uhi_info.di_signal.io_type_bit1 << 1);

    // zucmotStatus->di_type_pin_g2 = cab_internal_info.scb_di_uhi_info.di_signal.io_type_bit2 + (cab_internal_info.scb_di_uhi_info.di_signal.io_type_bit3 << 1);

    // zucmotStatus->do_type_pin_g1 =
    //     cab_internal_info.scb_do_ao_feedback_info.do_ao_feedback.io_type_bit0 + (cab_internal_info.scb_do_ao_feedback_info.do_ao_feedback.io_type_bit1 << 1);

    // zucmotStatus->do_type_pin_g2 =
    //     cab_internal_info.scb_do_ao_feedback_info.do_ao_feedback.io_type_bit2 + (cab_internal_info.scb_do_ao_feedback_info.do_ao_feedback.io_type_bit3 << 1);

    // // cabinet analog IO
    // if (zucmotConfig->cab_type == 1)
    // {
    //     for (aio = 0; aio < zucmotConfig->cabUsrPhysNumAI; aio++) { zucmotStatus->analog_input[aio] = zucmot_hal_data->physical_ai[aio]; }
    //     for (aio = 0; aio < zucmotConfig->cabUsrPhysNumAO; aio++) { zucmotStatus->analog_output[aio] = zucmot_hal_data->physical_ao[aio]; }
    // }
    // else
    // {
    //     if (zucmotConfig->cab_type == 2)
    //     {
    //         // if((zucmotStatus->synch_di[5]) && (first_di5_open == 1)/* && (!zucmotStatus->tracking) */){
    //         // 	rtapi_print(" ##################################################################### zucmotStatus->last_convayorCnt = %ld, zucmotStatus->synch_di[5] = %d #####################################################################\n",
    //         // 						zucmotStatus->last_convayorCnt, zucmotStatus->synch_di[5]);
    //         // 	zucmotStatus->turns_count = 0;
    //         // 	zucmotStatus->conveyorOrigin = zucmotStatus->last_convayorCnt;
    //         // 	first_di5_open = 0;
    //         // } else if(!zucmotStatus->synch_di[5]){
    //         // 	first_di5_open = 1;
    //         // }

    //         long tempVal;
    //         if ((cab_internal_info.scb_ao_ctl.ao_signal.ao_type & 0xf0) >= 0x90)  // analog_0 port type is INPUT
    //         {
    //             zucmotStatus->analog_input[0] =
    //                 (int)(cab_internal_info.scb_ai_uhi_info.ai_signal.ai_data_1_h << 8) + (int)(cab_internal_info.scb_ai_uhi_info.ai_signal.ai_data_1_l);
    //             zucmotStatus->analog_output[0] = 0;
    //         }
    //         else
    //         {
    //             zucmotStatus->analog_input[0] = 0;
    //             zucmotStatus->analog_output[0] = (int)(cab_internal_info.scb_do_ao_feedback_info.do_ao_feedback.ao_data_1_h << 8) +
    //                                              (int)(cab_internal_info.scb_do_ao_feedback_info.do_ao_feedback.ao_data_1_l);
    //         }

    //         if ((cab_internal_info.scb_ao_ctl.ao_signal.ao_type & 0x0f) >= 0x9)
    //         {
    //             zucmotStatus->analog_input[1] =
    //                 (int)(cab_internal_info.scb_ai_uhi_info.ai_signal.ai_data_2_h << 8) + (int)(cab_internal_info.scb_ai_uhi_info.ai_signal.ai_data_2_l);
    //             zucmotStatus->analog_output[1] = 0;
    //         }
    //         else
    //         {
    //             zucmotStatus->analog_input[1] = 0;
    //             zucmotStatus->analog_output[1] = (int)(cab_internal_info.scb_do_ao_feedback_info.do_ao_feedback.ao_data_2_h << 8) +
    //                                              (int)(cab_internal_info.scb_do_ao_feedback_info.do_ao_feedback.ao_data_2_l);
    //         }

    //         /*maintain the encoder move direction*/
    //         zucmotStatus->uhiData.dir = cab_internal_info.scb_ai_uhi_info.ai_signal.uhi_pules_direction;
    //         /*pulses calculation, maintain the turns*/
    //         tempVal = cab_internal_info.scb_ai_uhi_info.ai_signal.uhi_pules_h;
    //         tempVal = (tempVal << 8) + cab_internal_info.scb_ai_uhi_info.ai_signal.uhi_pules_l;

    //         // // for encoder simulation
    //         // static long tempVal = 0;
    //         // tempVal += 10;
    //         // tempVal = (tempVal % 65536);
    //         // cab_internal_info.scb_ai_uhi_info.ai_signal.uhi_pules_direction = 1;
    //         long int deltaCnt = 0;  //
    //         int i = 0;

    //         if (1 == cab_internal_info.scb_ai_uhi_info.ai_signal.uhi_pules_direction)
    //         {  //脉冲增加
    //             if (zucmotStatus->last_convayorCnt > tempVal)
    //             {
    //                 zucmotStatus->turns_count++;
    //                 deltaCnt = 65536 + tempVal - zucmotStatus->last_convayorCnt;
    //             }
    //             else
    //             {
    //                 deltaCnt = tempVal - zucmotStatus->last_convayorCnt;
    //             }
    //         }
    //         else if (2 == cab_internal_info.scb_ai_uhi_info.ai_signal.uhi_pules_direction)
    //         {  //脉冲减小
    //             if (zucmotStatus->last_convayorCnt < tempVal)
    //             {
    //                 zucmotStatus->turns_count--;
    //                 deltaCnt = 65536 + zucmotStatus->last_convayorCnt - tempVal;
    //             }
    //             else
    //             {
    //                 deltaCnt = zucmotStatus->last_convayorCnt - tempVal;
    //             }
    //         }

    //         if (zucmotConfig->com_cycle_num > TRACKING_MAX_COM_NUM)
    //         {
    //             zucmotConfig->com_cycle_num = TRACKING_MAX_COM_NUM;
    //         }
    //         for (i = 0; i < zucmotConfig->com_cycle_num; i++)
    //         {
    //             if (deltaCnts[i] == 0)
    //             {
    //                 deltaCnts[i] = deltaCnt;
    //                 break;
    //             }
    //         }
    //         if (i == zucmotConfig->com_cycle_num)
    //         {
    //             if (deltaCntIndex > (zucmotConfig->com_cycle_num - 1))
    //             {
    //                 deltaCntIndex = 0;
    //             }
    //             deltaCnts[deltaCntIndex] = deltaCnt;
    //             deltaCntIndex = deltaCntIndex + 1;
    //             long int sum = 0;
    //             for (int j = 0; j < zucmotConfig->com_cycle_num; j++) { sum += deltaCnts[j]; }
    //             if (zucmotConfig->com_cycle_num != 0)
    //             {
    //                 deltaCnt = ceil(sum / zucmotConfig->com_cycle_num);
    //             }
    //             else
    //             {
    //                 deltaCnt = 0;
    //             }
    //         }

    //         long lastPulses = zucmotStatus->uhiData.pulses;
    //         zucmotStatus->last_convayorCnt = tempVal;

    //         if ((zucmotConfig->link_di > 0) && (zucmotStatus->synch_di[zucmotConfig->link_di]) && (first_di5_open == 1) && (!zucmotStatus->tracking))
    //         {
    //             rtapi_print(" ##################################################################### zucmotStatus->last_convayorCnt = %ld, deltaCnt = %ld "
    //                         "#####################################################################\n",
    //                         zucmotStatus->last_convayorCnt,
    //                         deltaCnt);
    //             zucmotStatus->turns_count = 0;
    //             if (1 == cab_internal_info.scb_ai_uhi_info.ai_signal.uhi_pules_direction)
    //             {
    //                 zucmotStatus->conveyorOrigin = zucmotStatus->last_convayorCnt - 6 * deltaCnt;
    //                 if (zucmotStatus->conveyorOrigin < 0)
    //                 {
    //                     zucmotStatus->conveyorOrigin += 65536;
    //                     zucmotStatus->turns_count = 1;
    //                 }
    //             }
    //             else if (2 == cab_internal_info.scb_ai_uhi_info.ai_signal.uhi_pules_direction)
    //             {
    //                 zucmotStatus->conveyorOrigin = zucmotStatus->last_convayorCnt + 6 * deltaCnt;
    //                 if (zucmotStatus->conveyorOrigin > 65536)
    //                 {
    //                     zucmotStatus->conveyorOrigin -= 65536;
    //                     zucmotStatus->turns_count = -1;
    //                 }
    //             }
    //             first_di5_open = 0;
    //         }
    //         else if ((zucmotConfig->link_di > 0) && (!zucmotStatus->synch_di[zucmotConfig->link_di]))
    //         {
    //             first_di5_open = 1;
    //         }

    //         zucmotStatus->uhiData.pulses = zucmotStatus->turns_count * 65536 + tempVal;
    //         if (zucmotStatus->convyr_type == 1)
    //         {
    //             zucmotStatus->r_for_convyr = 1.0;
    //         }
    //         double delta_x = fabs(zucmotStatus->uhiData.pulses - lastPulses) * zucmotStatus->conveyorPulseEquivalent * zucmotStatus->r_for_convyr;
    //         double temp_v = delta_x / zucmotConfig->trajCycleTime;
    //         zucmot_axis_t* axis = &zucmotDebug->axes[0];
    //         if (temp_v > axis->vel_limit)
    //         {
    //             rtapi_print("===>> Abnormal encoder pulse during conveyor tracking: cur_pulse = %ld, last_pulse = %ld, temp_vel = %f, vel_limit = %f\n",
    //                         zucmotStatus->uhiData.pulses,
    //                         lastPulses,
    //                         temp_v,
    //                         axis->vel_limit);
    //             zucmotStatus->uhiData.pulses = lastPulses;
    //         }

    //         /*speed calculation*/
    //         tempVal = 0;
    //         tempVal = (tempVal << 8) + cab_internal_info.scb_di_uhi_info.di_signal.uhi_speed_lh;
    //         tempVal = (tempVal << 8) + cab_internal_info.scb_di_uhi_info.di_signal.uhi_speed_ll;
    //         zucmotStatus->uhiData.speed = zucmotStatus->last_convayorCnt;
    //         // // for encoder simulation
    //         // zucmotStatus->uhiData.speed = 50;
    //     }
    // }
}

void get_physic_IO_status()
{
    // if (zucmotConfig->cab_type == 1)
    // {
    //     // for (int di = 0; di < zucmotConfig->cabUsrPhysNumDIO; di++)
    //     // { write_DIO_i(IODevTYPE_SYNCH_MB_SLAVE, MB_INPT_GEN_PHY_DI_1 + di, zucmotStatus->synch_di[di]); }
    //     // for (int ai = 0; ai < ZUCMOT_MAX_MODBUS_PHYSIC_AI; ai++)
    //     // { write_AIO_f(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_CAB_PHY_AI_1 + ai, zucmotStatus->analog_input[ai]); }
    //     for (int di = 0; di < zucmotConfig->cabUsrPhysNumDIO; di++)
    //     {
    //         zucmotExtIOStruct->MbSlaveShmStruct.discrete_input[MB_INPT_GEN_PHY_DI_1 + di] = zucmotStatus->synch_di[di];
    //     }
    //     for (int ai = 0; ai < ZUCMOT_MAX_MODBUS_PHYSIC_AI; ai++)
    //     {
    //         zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_CAB_PHY_AI_1 + ai] = zucmotStatus->analog_input[ai];
    //     }
    // }
    // else if (zucmotConfig->cab_type == 2)
    // {
    //     // cab io status
    //     // for (int di = 0; di < ZUCMOT_SCB_USER_DIO_NUM; di++)
    //     // { write_DIO_i(IODevTYPE_SYNCH_MB_SLAVE, MB_INPT_GEN_PHY_DI_1 + di, zucmotStatus->synch_di[di]); }
    //     // for (int ai = 0; ai < ZUCMOT_MAX_MODBUS_PHYSIC_AI; ai++)
    //     // { write_AIO_f(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_CAB_PHY_AI_1 + ai, zucmotStatus->analog_input[ai]); }
    //     for (int di = 0; di < ZUCMOT_SCB_USER_DIO_NUM; di++)
    //     {
    //         zucmotExtIOStruct->MbSlaveShmStruct.discrete_input[MB_INPT_GEN_PHY_DI_1 + di] = zucmotStatus->synch_di[di];
    //     }

    //     for (int ai = 0; ai < ZUCMOT_MAX_MODBUS_PHYSIC_AI; ai++)
    //     {
    //         zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_CAB_PHY_AI_1 + ai] = zucmotStatus->analog_input[ai];
    //     }
    // }
    // else if (zucmotConfig->cab_type == 3)
    // {
    //     // for (int di = 0; di < ZUCMOT_MINI_USER_DIO_NUM; di++)
    //     // { write_DIO_i(IODevTYPE_SYNCH_MB_SLAVE, MB_INPT_GEN_PHY_DI_1 + di, zucmotStatus->synch_di[di]); }
    //     for (int di = 0; di < ZUCMOT_MINI_USER_DIO_NUM; di++)
    //     {
    //         zucmotExtIOStruct->MbSlaveShmStruct.discrete_input[MB_INPT_GEN_PHY_DI_1 + di] = zucmotStatus->synch_di[di];
    //     }
    // }

    // // tio di
    // for (int tio_di = 0; tio_di < 2; tio_di++)
    // {
    //     // int tmp = (zucmotStatus->tio_status.DI_buffer & (1 << tio_di)) > 0 ? 1 : 0;
    //     // write_DIO_i(IODevTYPE_SYNCH_MB_SLAVE, MB_INPT_GEN_PHY_TOOL_DI_1 + tio_di, tmp);
    //     zucmotExtIOStruct->MbSlaveShmStruct.discrete_input[MB_INPT_GEN_PHY_TOOL_DI_1 + tio_di] =
    //         (zucmotStatus->tio_status.DI_buffer & (1 << tio_di)) > 0 ? 1 : 0;
    // }

    // //tio ai
    // // for (int ai = 0; ai < ZUCMOT_MAX_MODBUS_PHYSIC_AI; ai++){
    // // write_AIO_f(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_TOOL_PHY_AI_1 + ai, zucmotStatus->tio_status.AI[ai]);}
    // for (int ai = 0; ai < ZUCMOT_MAX_MODBUS_PHYSIC_AI; ai++)
    // {
    //     zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_TOOL_PHY_AI_1 + ai] = zucmotStatus->tio_status.AI[ai];
    // }
}

void update_mb_IO()
{
    // int dio, aio;
    // // modbus digital IO
    // for (dio = 0; dio < ZUCMOT_MAX_MODBUS_DIO; dio++)
    // {
    //     Read_DIOStatus_i(IODevTYPE_SYNCH_MB_SLAVE, ADIO_INPUT, dio, &zucmotStatus->synch_mb_slave_di[dio]);
    //     Read_DIOStatus_i(IODevTYPE_SYNCH_MB_SLAVE, ADIO_OUTPUT, dio, &zucmotStatus->synch_mb_slave_do[dio]);
    //     // zucmotStatus->synch_mb_slave_di[dio] = zucmotExtIOStruct->MbSlaveShmStruct.coils[MB_COIL_GEN_0 + dio];  // IO Type = 4 for Modbus DIO
    //     // zucmotStatus->synch_mb_slave_do[dio] = zucmotExtIOStruct->MbSlaveShmStruct.discrete_input[MB_D_INPT_GEN_0 + dio];

    //     Read_DIOStatus_i(IODevTYPE_SYNCH_MB_SLAVE, ADIO_INPUT, dio, &zucmotStatus->synch_di[dio + zucmotConfig->cabUsrPhysNumDIO]);
    //     Read_DIOStatus_i(IODevTYPE_SYNCH_MB_SLAVE, ADIO_OUTPUT, dio, &zucmotStatus->synch_do[dio + zucmotConfig->cabUsrPhysNumDIO]);
    //     // zucmotStatus->synch_di[dio + zucmotConfig->cabUsrPhysNumDIO] =
    //     //     zucmotExtIOStruct->MbSlaveShmStruct.coils[MB_COIL_GEN_0 + dio];  // IO Type = 0 for Modbus DIO
    //     // zucmotStatus->synch_do[dio + zucmotConfig->cabUsrPhysNumDIO] = zucmotExtIOStruct->MbSlaveShmStruct.discrete_input[MB_D_INPT_GEN_0 + dio];
    // }

    // for (int i = 0; i < ZUCMOT_MAX_MODBUS_AIO_NUM; i++)
    // {
    //     Read_AIOStatus_f(IODevTYPE_SYNCH_MB_SLAVE, ADIO_INPUT, i, &zucmotStatus->analog_mb_slave_input[i]);
    //     Read_AIOStatus_f(IODevTYPE_SYNCH_MB_SLAVE, ADIO_OUTPUT, i, &zucmotStatus->analog_mb_slave_output[i]);

    //     Read_AIOStatus_f(IODevTYPE_SYNCH_MB_SLAVE, ADIO_INPUT, i, &zucmotStatus->analog_input[i + zucmotConfig->cabUsrPhysNumAI]);
    //     Read_AIOStatus_f(IODevTYPE_SYNCH_MB_SLAVE, ADIO_OUTPUT, i, &zucmotStatus->analog_output[i + zucmotConfig->cabUsrPhysNumAO]);
    // }
}

void update_mb_status()
{
    // zucmot_joint_t* joint;
    // unsigned short* temp_reg = NULL;
    // int orginal_uhi_data = 0;
    // for (int idx = 0; idx < 6; idx++)
    // {
    //     joint = &zucmotDebug->joints[idx];

    //     write_Joint_f(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_REG_JNT_ANGL_1_H + 2 * idx, joint->pos_cmd);
    //     // temp_reg = float32_littleendian_to_bigendian(joint->pos_cmd);
    //     // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_JNT_ANGL_1_H + 2 * idx] = temp_reg[0];
    //     // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_JNT_ANGL_1_L + 2 * idx] = temp_reg[1];

    //     write_Joint_f(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_REG_JNT_SPD_1_H + 2 * idx, joint->vel_cmd);
    //     // temp_reg = float32_littleendian_to_bigendian(joint->vel_cmd);
    //     // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_JNT_SPD_1_H + 2 * idx] = temp_reg[0];
    //     // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_JNT_SPD_1_L + 2 * idx] = temp_reg[1];

    //     write_Joint_f(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_REG_JNT_CRNT_1_H + 2 * idx, zucmotStatus->servo_status.jointMonitorData[idx].instCurrent);
    //     // temp_reg = float32_littleendian_to_bigendian(zucmotStatus->servo_status.jointMonitorData[idx].instCurrent);
    //     // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_JNT_CRNT_1_H + 2 * idx] = temp_reg[0];
    //     // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_JNT_CRNT_1_L + 2 * idx] = temp_reg[1];

    //     write_Joint_f(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_REG_ACT_TORQUE_1_H + 2 * idx, zucmotStatus->actualTorque[idx]);
    //     // temp_reg = float32_littleendian_to_bigendian(zucmotStatus->actualTorque[idx]);
    //     // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_ACT_TORQUE_1_H + 2 * idx] = temp_reg[0];
    //     // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_ACT_TORQUE_1_L + 2 * idx] = temp_reg[1];

    //     if (((zucmot_hal_data->error_triggered == true && (zucmotStatus->servo_status.errorCode[idx] != 0)) ||
    //          (zucmot_hal_data->error_triggered == false && (zucmotStatus->servo_status.errorCode[idx] == 0))))
    //     {
    //         write_Joint_f(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_REG_JNT_ERR_CODE_1_H + 2 * idx, zucmotStatus->servo_status.errorCode[idx]);
    //         // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_JNT_ERR_CODE_1_H + 2 * idx] =
    //         //     (unsigned short)(zucmotStatus->servo_status.errorCode[idx] >> 16);
    //         // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_JNT_ERR_CODE_1_L + 2 * idx] = (unsigned short)(zucmotStatus->servo_status.errorCode[idx]);
    //     }
    //     if (((zucmot_hal_data->error_triggered == true && (zucmotStatus->servo_status.collisionState[idx] != 0)) ||
    //          (zucmot_hal_data->error_triggered == false && (zucmotStatus->servo_status.collisionState[idx] == 0))))
    //     {
    //         write_Joint_uint8(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_REG_JNT_CLSN_STS_1 + idx, zucmotStatus->servo_status.collisionState[idx]);
    //         // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_JNT_CLSN_STS_1 + idx] = (unsigned short)(zucmotStatus->servo_status.collisionState[idx]);
    //     }

    //     write_Joint_f(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_REG_JNT_PWR_1_H + 2 * idx, zucmotStatus->servo_status.jointMonitorData[idx].instVoltage);
    //     // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_JNT_PWR_1_H + 2 * idx] =
    //     //     (unsigned short)(((*(unsigned int*)&zucmotStatus->servo_status.jointMonitorData[idx].instVoltage) >> 16) & 0xffff);
    //     // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_JNT_PWR_1_L + 2 * idx] =
    //     //     (unsigned short)(((*(unsigned int*)&zucmotStatus->servo_status.jointMonitorData[idx].instVoltage)) & 0xffff);

    //     write_Joint_f(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_REG_ACT_TORQUE_1_H + 2 * idx, zucmotStatus->servo_status.jointMonitorData[idx].instTemperature);
    //     // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_JNT_TMP_1_H + 2 * idx] =
    //     //     (unsigned short)(((*(unsigned int*)&zucmotStatus->servo_status.jointMonitorData[idx].instTemperature) >> 16) & 0xffff);
    //     // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_JNT_TMP_1_H + 2 * idx] =
    //     //     (unsigned short)(((*(unsigned int*)&zucmotStatus->servo_status.jointMonitorData[idx].instTemperature)) & 0xffff);

    //     write_Joint_uint8(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_REG_JNT_ERR_STS_1 + idx, zucmotStatus->servo_status.errorState[idx]);
    //     // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_JNT_ERR_STS_1 + idx] = (unsigned short)(zucmotStatus->servo_status.errorState[idx]);
    //     write_Joint_uint8(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_REG_JNT_EN_STS_1 + idx, zucmotStatus->servo_status.enableState[idx]);
    //     // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_JNT_EN_STS_1 + idx] = (unsigned short)(zucmotStatus->servo_status.enableState[idx]);
    // }

    // for (int idx = 0; idx < 6; idx++)
    // {
    //     write_TCP_f(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_REG_TCP_SPD_X_H + 2 * idx, zucmotStatus->vel_tcp[idx]);
    //     // temp_reg = float32_littleendian_to_bigendian(zucmotStatus->vel_tcp[idx]);
    //     // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_TCP_SPD_X_H + 2 * idx] = temp_reg[0];
    //     // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_TCP_SPD_X_L + 2 * idx] = temp_reg[1];

    //     write_TCP_f(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_REG_TCP_POS_X_H + 2 * idx, zucmotStatus->tcp_pos[idx]);
    //     // temp_reg = float32_littleendian_to_bigendian(zucmotStatus->tcp_pos[idx]);
    //     // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_TCP_POS_X_H + 2 * idx] = temp_reg[0];
    //     // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_TCP_POS_X_L + 2 * idx] = temp_reg[1];

    //     write_ToolOffset_f(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_REG_TCP_CUR_OFFSET_X_H + 2 * idx, zucmotStatus->tool_offset[idx]);
    //     // temp_reg = float32_littleendian_to_bigendian(zucmotStatus->tool_offset[idx]);
    //     // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_TCP_CUR_OFFSET_X_H + 2 * idx] = temp_reg[0];
    //     // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_TCP_CUR_OFFSET_X_L + 2 * idx] = temp_reg[1];

    //     write_BaseOffset_f(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_REG_BASE_OFFSET_X_H + 2 * idx, zucmotStatus->user_offset[idx]);
    //     // temp_reg = float32_littleendian_to_bigendian(zucmotStatus->user_offset[idx]);
    //     // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_BASE_OFFSET_X_H + 2 * idx] = temp_reg[0];
    //     // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_BASE_OFFSET_X_L + 2 * idx] = temp_reg[1];
    // }

    // write_Robot_i(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_REG_SERVO_VER_1_H, zucmotStatus->servo_status.servo_version);
    // // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_SERVO_VER_1_H] = (unsigned short)(zucmotStatus->servo_status.servo_version >> 16);
    // // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_SERVO_VER_1_L] = (unsigned short)(zucmotStatus->servo_status.servo_version);

    // write_Robot_i(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_REG_ROBOT_SN_1_H, zucmotStatus->servo_status.serial_num);
    // // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_ROBOT_SN_1_H] = (unsigned short)(zucmotStatus->servo_status.serial_num >> 16);
    // // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_ROBOT_SN_1_L] = (unsigned short)(zucmotStatus->servo_status.serial_num);

    // write_SafetyOUT_uint8(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_REG_PERCENTAGE_MODE_LEVEL, zucmotStatus->percentage_mode_level);
    // write_SafetyOUT_uint8(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_REG_PROTECTIVE_STOP, zucmotStatus->protective_stop);
    // write_SafetyOUT_uint8(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_REG_EMERGENCY_STOP, zucmotStatus->emergency_stop);
    // write_SafetyOUT_uint8(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_REG_ON_SOFT_LIMIT, zucmotStatus->on_soft_limit);

    // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_PERCENTAGE_MODE_LEVEL] = zucmotStatus->percentage_mode_level;
    // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_PROTECTIVE_STOP] = zucmotStatus->protective_stop;
    // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_EMERGENCY_STOP] = zucmotStatus->emergency_stop;
    // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_ON_SOFT_LIMIT] = zucmotStatus->on_soft_limit;
    // /*motion mode*/

    // write_Robot_uint8(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_REG_POWER_ON, zucmotStatus->powered_on);
    // write_Robot_uint8(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_REG_ENABLE, GET_MOTION_ENABLE_FLAG());
    // write_Robot_uint8(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_REG_INPOS, GET_MOTION_INPOS_FLAG());
    // write_Robot_uint8(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_REG_MOTION_MODE, zucmotStatus->motion_mode);

    // // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_POWER_ON] = zucmotStatus->powered_on;
    // // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_ENABLE] = GET_MOTION_ENABLE_FLAG();
    // // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_INPOS] = GET_MOTION_INPOS_FLAG();
    // // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_MOTION_MODE] = zucmotStatus->motion_mode;

    // write_Robot_i(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_REG_MOTION_ERRCODE_H, zucmot_hal_data->motion_error_code);
    // // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_MOTION_ERRCODE_H] = (unsigned short)(((int)*(zucmot_hal_data->motion_error_code)) >> 16);
    // // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_MOTION_ERRCODE_L] = (unsigned short)((int)*(zucmot_hal_data->motion_error_code));

    // write_Robot_f(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_REG_RAPID_SCALE_H, zucmotStatus->rapid_scale);
    // // temp_reg = float32_littleendian_to_bigendian(zucmotStatus->rapid_scale);
    // // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_RAPID_SCALE_H] = temp_reg[0];
    // // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_RAPID_SCALE_L] = temp_reg[1];

    // // robot
    // write_Robot_f(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_REG_CAB_TEMPERATURE_H, zucmotStatus->cab_status.cabTemperature);
    // // temp_reg = float32_littleendian_to_bigendian(zucmotStatus->cab_status.cabTemperature);
    // // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_CAB_TEMPERATURE_H] = temp_reg[0];
    // // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_CAB_TEMPERATURE_L] = temp_reg[1];

    // write_Robot_f(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_REG_ROBOT_AVERAGECURRENT_H, zucmotStatus->cab_status.robotAverageCurrent);
    // // temp_reg = float32_littleendian_to_bigendian(zucmotStatus->cab_status.robotAverageCurrent);
    // // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_ROBOT_AVERAGECURRENT_H] = temp_reg[0];
    // // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_ROBOT_AVERAGECURRENT_L] = temp_reg[1];

    // write_Robot_f(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_REG_ROBOT_AVERAGEPOWER_H, zucmotStatus->cab_status.robotAveragePower);
    // // temp_reg = float32_littleendian_to_bigendian(zucmotStatus->cab_status.robotAveragePower);
    // // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_ROBOT_AVERAGEPOWER_H] = temp_reg[0];
    // // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_ROBOT_AVERAGEPOWER_L] = temp_reg[1];

    // /*uhi data*/
    // write_Uhi_uint8(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_REG_UHI_DIR, zucmotStatus->uhiData.dir);
    // // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_UHI_DIR] = zucmotStatus->uhiData.dir;

    // write_Uhi_f(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_REG_UHI_PULES_H, zucmotStatus->uhiData.pulses);
    // // temp_reg = int32_littleendian_to_bigendian(zucmotStatus->uhiData.pulses);
    // // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_UHI_PULES_H] = temp_reg[0];
    // // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_UHI_PULES_L] = temp_reg[1];

    // write_Uhi_f(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_REG_UHI_SPEED_H, zucmotStatus->uhiData.speed);
    // // temp_reg = int32_littleendian_to_bigendian(zucmotStatus->uhiData.speed);
    // // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_UHI_SPEED_H] = temp_reg[0];
    // // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_UHI_SPEED_L] = temp_reg[1];

    // orginal_uhi_data = cab_internal_info.scb_ai_uhi_info.ai_signal.uhi_pules_h;
    // orginal_uhi_data = (orginal_uhi_data << 8) + cab_internal_info.scb_ai_uhi_info.ai_signal.uhi_pules_l;

    // write_Uhi_int32(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_REG_UHI_ORIGIN_PULES_H, orginal_uhi_data);
    // // temp_reg = int32_littleendian_to_bigendian(orginal_uhi_data);
    // // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_UHI_ORIGIN_PULES_H] = temp_reg[0];
    // // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_UHI_ORIGIN_PULES_L] = temp_reg[1];

    // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_ERROR_TRIGGERED] = zucmot_hal_data->error_triggered;

    // write_Robot_f(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_REG_TCP_SPD_H, zucmotStatus->vel_tcp_linear);

    // write_Robot_f(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_REG_TCP_ANG_H, zucmotStatus->vel_tcp_angular);

    // /*reserved*/
    // write_Robot_i(IODevTYPE_SYNCH_MB_SLAVE, MB_IN_REG_CHECKSUM_H, zucmotStatus->safetyCRC);

    // // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_RESERVE_1] = 0;
    // // zucmotExtIOStruct->MbSlaveShmStruct.input_reg[MB_IN_REG_RESERVE_2] = 0;
}